#include "Inverse_kinematics.h"
#include <math.h>
#include <stdio.h>

/* 结构参数（单位：m）——采用 Matlab 文件中的值 */
static const double L1 = 148e-3;
static const double L2 = 42e-3;
static const double L3 = 132e-3;
static const double L4 = 122e-3;
static const double L5 = 108e-3;
static const double L6 = 78e-3; /* 与 Matlab 一致：0.078 m */

/* 转换比例（与 Matlab 中 electronic_gear_ratio 一致） */
static const double POSITION_PULSE_PER_M =
    1.0 / 50e-9; /* 位移：1 脉冲对应 50 nm */
static const double ROTATIONAL_PULSE_PER_DEG_DD =
    1.0 / 0.001; /* 力矩电机：1 脉冲对应 0.001° */
static const double ROTATIONAL_PULSE_PER_DEG_ZOLIX =
    1.0 / 0.0002475921753; /* Zolix 电机 */

/* 零点脉冲（示例值，实物请以测量为准） */
extern int ZERO_PULSE_THETA6;
static const int ZERO_PULSE_THETA5 = 71158;
static const int ZERO_PULSE_D4 = 795823;
static const int ZERO_PULSE_D3 = 703125;
static const int ZERO_PULSE_D2 = 705678;
static const int ZERO_PULSE_THETA1 = 226148;

/* 矩阵乘法工具函数 */
static void matrix_multiply_3x3(const double A[3][3], const double B[3][3],
                                double C[3][3]) {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      C[i][j] = 0.0;
      for (int k = 0; k < 3; k++) {
        C[i][j] += A[i][k] * B[k][j];
      }
    }
  }
}

/* 矩阵乘向量工具函数 */
static void matrix_vector_multiply_3x3(const double A[3][3], const double v[3],
                                       double result[3]) {
  for (int i = 0; i < 3; i++) {
    result[i] = 0.0;
    for (int k = 0; k < 3; k++) {
      result[i] += A[i][k] * v[k];
    }
  }
}

/* 4x4齐次变换矩阵乘法 */
static void homogeneous_multiply_4x4(const double A[4][4], const double B[4][4],
                                     double C[4][4]) {
  for (int i = 0; i < 4; i++) {
    for (int j = 0; j < 4; j++) {
      C[i][j] = 0.0;
      for (int k = 0; k < 4; k++) {
        C[i][j] += A[i][k] * B[k][j];
      }
    }
  }
}

/* 构建齐次变换矩阵 */
static void build_homogeneous_transform(const double R[3][3], const double P[3],
                                        double T[4][4]) {
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      T[i][j] = R[i][j];
    }
    T[i][3] = P[i];
  }
  T[3][0] = T[3][1] = T[3][2] = 0.0;
  T[3][3] = 1.0;
}

/* 计算齐次变换矩阵的逆 T_inv = inv(T) - 已修正版本 */
static void homogeneous_inverse_4x4(const double T[4][4], double T_inv[4][4]) {
  /* 对于齐次变换矩阵 T = [R, P; 0, 1]
   * 其逆矩阵为 T_inv = [R^T, -R^T*P; 0, 1] */

  // 1. 创建一个临时的、类型正确的 3x3 矩阵来存储 R^T
  double R_T[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R_T[i][j] = T[j][i];
    }
  }

  // 2. 将计算好的 R_T 复制到 T_inv 的对应位置
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      T_inv[i][j] = R_T[i][j];
    }
  }

  // 3. 提取位移向量 P
  double P[3] = {T[0][3], T[1][3], T[2][3]};
  double RT_P[3];

  // 4. 使用类型正确的 R_T 矩阵进行乘法运算，避免强制类型转换
  matrix_vector_multiply_3x3(R_T, P, RT_P);

  // 5. 计算最终的位移部分
  for (int i = 0; i < 3; i++) {
    T_inv[i][3] = -RT_P[i];
  }

  // 6. 填充底行
  T_inv[3][0] = T_inv[3][1] = T_inv[3][2] = 0.0;
  T_inv[3][3] = 1.0;
}

/* 内部实现：原始的绝对位置逆运动学 */
static void ideal_DH_IK(const double parameter[6], double x_um, double y_um,
                        double z_um, double rx_deg, double ry_deg,
                        double rz_deg, double *theta1_deg, double *d2_m,
                        double *d3_m, double *d4_m, double *theta5_deg,
                        double *theta6_deg) {
  /* parameter: parameter[0..2] in μm, parameter[3..5] in degrees
     obj pos: x_um,y_um,z_um in μm; rx,ry,rz in degrees */

  // 在函数一开始就打印，确认传入的值是否正确
  printf("[IK ENTRY DEBUG] Received obj_pos(x,y,z) = (%.1f, %.1f, %.1f)\n",
         x_um, y_um, z_um);

  /* 转换为 SI 单位（m / rad） */
  double P_T0_x = parameter[0] * 1e-6;
  double P_T0_y = parameter[1] * 1e-6;
  double P_T0_z = parameter[2] * 1e-6;
  double alphaT = parameter[3] * (M_PI / 180.0);
  double betaT = parameter[4] * (M_PI / 180.0);
  double gammaT = parameter[5] * (M_PI / 180.0);

  /* obj pos 转为 m（由 μm -> m）*/
  double P_Te_x = x_um * 1e-6;
  double P_Te_y = y_um * 1e-6;
  double P_Te_z = z_um * 1e-6;

  /* 角度 -> 弧度 */
  double rx = rx_deg * (M_PI / 180.0);
  double ry = ry_deg * (M_PI / 180.0);
  double rz = rz_deg * (M_PI / 180.0);

  /* 构造 R_T0 = Rx(alphaT) * Ry(betaT) * Rz(gammaT) */
  double RxT[3][3] = {
      {1, 0, 0}, {0, cos(alphaT), -sin(alphaT)}, {0, sin(alphaT), cos(alphaT)}};
  double RyT[3][3] = {
      {cos(betaT), 0, sin(betaT)}, {0, 1, 0}, {-sin(betaT), 0, cos(betaT)}};
  double RzT[3][3] = {
      {cos(gammaT), -sin(gammaT), 0}, {sin(gammaT), cos(gammaT), 0}, {0, 0, 1}};
  double temp1[3][3], R_T0[3][3];
  matrix_multiply_3x3(RxT, RyT, temp1);
  matrix_multiply_3x3(temp1, RzT, R_T0);

  /* R_Te = Rx(rx) * Ry(ry) * Rz(rz) */
  double RxE[3][3] = {{1, 0, 0}, {0, cos(rx), -sin(rx)}, {0, sin(rx), cos(rx)}};
  double RyE[3][3] = {{cos(ry), 0, sin(ry)}, {0, 1, 0}, {-sin(ry), 0, cos(ry)}};
  double RzE[3][3] = {{cos(rz), -sin(rz), 0}, {sin(rz), cos(rz), 0}, {0, 0, 1}};
  double temp2[3][3], R_Te[3][3];
  matrix_multiply_3x3(RxE, RyE, temp2);
  matrix_multiply_3x3(temp2, RzE, R_Te);

  /* 构建齐次变换矩阵 */
  double P_T0[3] = {P_T0_x, P_T0_y, P_T0_z};
  double P_Te[3] = {P_Te_x, P_Te_y, P_Te_z};
  double T_T0[4][4], T_Te[4][4];

  build_homogeneous_transform(R_T0, P_T0, T_T0);
  build_homogeneous_transform(R_Te, P_Te, T_Te);

  // // ******** 添加第一段调试代码 ********
  // printf("DEBUG T_Te Matrix for obj_pos(y,z)=(%.1f, %.1f):\n", y_um, z_um);
  // for (int i = 0; i < 4; i++) {
  //   printf("  [ %.6f, %.6f, %.6f, %.6f ]\n", T_Te[i][0], T_Te[i][1],
  //   T_Te[i][2],
  //          T_Te[i][3]);
  // }
  // // ******** 结束第一段调试代码 ********

  /* T_e0 = T_T0 * inv(T_Te) */
  double T_Te_inv[4][4], T_e0[4][4];
  homogeneous_inverse_4x4(T_Te, T_Te_inv);

  // // ******** 添加第二段调试代码 ********
  // printf("DEBUG T_Te_inv Matrix for obj_pos(y,z)=(%.1f, %.1f):\n", y_um,
  // z_um); for (int i = 0; i < 4; i++) {
  //   printf("  [ %.6f, %.6f, %.6f, %.6f ]\n", T_Te_inv[i][0], T_Te_inv[i][1],
  //          T_Te_inv[i][2], T_Te_inv[i][3]);
  // }
  // // ******** 结束第二段调试代码 ********

  homogeneous_multiply_4x4(T_T0, T_Te_inv, T_e0);

  // // ******** 开始添加新的调试代码 ********
  // printf("DEBUG T_e0 Matrix for obj_pos(y,z)=(%.1f, %.1f):\n", y_um, z_um);
  // for (int i = 0; i < 4; i++) {
  //   printf("  [ %.6f, %.6f, %.6f, %.6f ]\n", T_e0[i][0], T_e0[i][1],
  //   T_e0[i][2],
  //          T_e0[i][3]);
  // }
  // // ******** 结束添加新的调试代码 ********

  /* 提取旋转矩阵和位置向量 */
  double R_e0[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R_e0[i][j] = T_e0[i][j];
    }
  }
  double xe0 = T_e0[0][3];
  double ye0 = T_e0[1][3];
  double ze0 = T_e0[2][3];

  /* 从 R_e0 中提取角（与 Matlab 完全一致的公式） */
  double t1 = atan2(-R_e0[0][1], R_e0[1][1]);
  double sin_t5 = R_e0[2][1];
  if (sin_t5 > 1.0)
    sin_t5 = 1.0;
  if (sin_t5 < -1.0)
    sin_t5 = -1.0;
  double t5 = asin(sin_t5);
  double t6 = -atan2(-R_e0[2][0], R_e0[2][2]);

  /* 计算 d2,d3,d4（与 Matlab 一致） */
  double d2 = ye0 * cos(t1) - xe0 * sin(t1) - L6 * cos(t6) * sin(t5);
  double d3 = -L4 - L5 + xe0 * cos(t1) + ye0 * sin(t1) - L6 * sin(t6);
  double d4 = -L1 - L2 - L3 + ze0 + L6 * cos(t5) * cos(t6);
  printf("[IK DEBUG] obj_pos(y,z)=(%.1f, %.1f) -> d2_m=%.6f, d3_m=%.6f, "
         "d4_m=%.6f， t1=%.6f, t5=%.6f, t6=%.6f",
         y_um, z_um, d2, d3, d4, t1 * (180.0 / M_PI), t5 * (180.0 / M_PI),
         t6 * (180.0 / M_PI));

  /* 输出：角度 -> deg，d* -> m */
  *theta1_deg = t1 * (180.0 / M_PI);
  *theta5_deg = t5 * (180.0 / M_PI);
  *theta6_deg = t6 * (180.0 / M_PI);
  *d2_m = d2;
  *d3_m = d3;
  *d4_m = d4;
}

/* 新增：增量式逆运动学函数 */
static void incremental_ideal_DH_IK(const double parameter[6],
                                    const double parameter_FK[6], double x_um,
                                    double y_um, double z_um, double rx_deg,
                                    double ry_deg, double rz_deg,
                                    double *theta1_deg, double *d2_m,
                                    double *d3_m, double *d4_m,
                                    double *theta5_deg, double *theta6_deg) {
  /* 构建当前工具坐标系到基坐标系的变换矩阵 (T_T0_current) */
  double alphaT_current = parameter_FK[3] * (M_PI / 180.0);
  double betaT_current = parameter_FK[4] * (M_PI / 180.0);
  double gammaT_current = parameter_FK[5] * (M_PI / 180.0);

  double RxT_current[3][3] = {{1, 0, 0},
                              {0, cos(alphaT_current), -sin(alphaT_current)},
                              {0, sin(alphaT_current), cos(alphaT_current)}};
  double RyT_current[3][3] = {{cos(betaT_current), 0, sin(betaT_current)},
                              {0, 1, 0},
                              {-sin(betaT_current), 0, cos(betaT_current)}};
  double RzT_current[3][3] = {{cos(gammaT_current), -sin(gammaT_current), 0},
                              {sin(gammaT_current), cos(gammaT_current), 0},
                              {0, 0, 1}};

  double temp_current[3][3], R_T0_current[3][3];
  matrix_multiply_3x3(RxT_current, RyT_current, temp_current);
  matrix_multiply_3x3(temp_current, RzT_current, R_T0_current);

  double P_T0_current[3] = {parameter_FK[0] * 1e-6, parameter_FK[1] * 1e-6,
                            parameter_FK[2] * 1e-6};

  double T_T0_current[4][4];
  build_homogeneous_transform(R_T0_current, P_T0_current, T_T0_current);

  /* 构建增量变换矩阵 (T_T0_incremental) */
  double alphaT_inc = parameter[3] * (M_PI / 180.0);
  double betaT_inc = parameter[4] * (M_PI / 180.0);
  double gammaT_inc = parameter[5] * (M_PI / 180.0);

  double RxT_inc[3][3] = {{1, 0, 0},
                          {0, cos(alphaT_inc), -sin(alphaT_inc)},
                          {0, sin(alphaT_inc), cos(alphaT_inc)}};
  double RyT_inc[3][3] = {{cos(betaT_inc), 0, sin(betaT_inc)},
                          {0, 1, 0},
                          {-sin(betaT_inc), 0, cos(betaT_inc)}};
  double RzT_inc[3][3] = {{cos(gammaT_inc), -sin(gammaT_inc), 0},
                          {sin(gammaT_inc), cos(gammaT_inc), 0},
                          {0, 0, 1}};

  double temp_inc[3][3], R_T0_inc[3][3];
  matrix_multiply_3x3(RxT_inc, RyT_inc, temp_inc);
  matrix_multiply_3x3(temp_inc, RzT_inc, R_T0_inc);

  double P_T0_inc[3] = {parameter[0] * 1e-6, parameter[1] * 1e-6,
                        parameter[2] * 1e-6};

  double T_T0_incremental[4][4];
  build_homogeneous_transform(R_T0_inc, P_T0_inc, T_T0_incremental);

  /* 计算目标变换矩阵: T_T0_objective = T_T0_incremental * T_T0_current */
  double T_T0_objective[4][4];
  homogeneous_multiply_4x4(T_T0_incremental, T_T0_current, T_T0_objective);

  /* 构建工具到机械臂末端的变换矩阵 T_Te */
  double rx = rx_deg * (M_PI / 180.0);
  double ry = ry_deg * (M_PI / 180.0);
  double rz = rz_deg * (M_PI / 180.0);

  double RxE[3][3] = {{1, 0, 0}, {0, cos(rx), -sin(rx)}, {0, sin(rx), cos(rx)}};
  double RyE[3][3] = {{cos(ry), 0, sin(ry)}, {0, 1, 0}, {-sin(ry), 0, cos(ry)}};
  double RzE[3][3] = {{cos(rz), -sin(rz), 0}, {sin(rz), cos(rz), 0}, {0, 0, 1}};

  double temp_e[3][3], R_Te[3][3];
  matrix_multiply_3x3(RxE, RyE, temp_e);
  matrix_multiply_3x3(temp_e, RzE, R_Te);

  double P_Te[3] = {x_um * 1e-6, y_um * 1e-6, z_um * 1e-6};
  double T_Te[4][4];
  build_homogeneous_transform(R_Te, P_Te, T_Te);

  /* T_e0 = T_T0_objective * inv(T_Te) */
  double T_Te_inv[4][4], T_e0[4][4];
  homogeneous_inverse_4x4(T_Te, T_Te_inv);
  homogeneous_multiply_4x4(T_T0_objective, T_Te_inv, T_e0);

  /* 提取旋转矩阵和位置向量 */
  double R_e0[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      R_e0[i][j] = T_e0[i][j];
    }
  }
  double xe0 = T_e0[0][3];
  double ye0 = T_e0[1][3];
  double ze0 = T_e0[2][3];

  /* 逆运动学求解（与绝对位置版本相同） */
  double t1 = atan2(-R_e0[0][1], R_e0[1][1]);
  double sin_t5 = R_e0[2][1];
  if (sin_t5 > 1.0)
    sin_t5 = 1.0;
  if (sin_t5 < -1.0)
    sin_t5 = -1.0;
  double t5 = asin(sin_t5);
  double t6 = -atan2(-R_e0[2][0], R_e0[2][2]);

  double d2 = ye0 * cos(t1) - xe0 * sin(t1) - L6 * cos(t6) * sin(t5);
  double d3 = -L4 - L5 + xe0 * cos(t1) + ye0 * sin(t1) - L6 * sin(t6);
  double d4 = -L1 - L2 - L3 + ze0 + L6 * cos(t5) * cos(t6);

  /* 输出 */
  *theta1_deg = t1 * (180.0 / M_PI);
  *theta5_deg = t5 * (180.0 / M_PI);
  *theta6_deg = t6 * (180.0 / M_PI);
  *d2_m = d2;
  *d3_m = d3;
  *d4_m = d4;
}

/* 原有的绝对位置逆运动学接口 */
int inverse_kinematics(const double parameter[6], double x_um, double y_um,
                       double z_um, double rx_deg, double ry_deg, double rz_deg,
                       int *pulse_theta1, int *pulse_d2, int *pulse_d3,
                       int *pulse_d4, int *pulse_theta5, int *pulse_theta6) {
  double theta1_deg = 0.0, theta5_deg = 0.0, theta6_deg = 0.0;
  double d2_m = 0.0, d3_m = 0.0, d4_m = 0.0;

  ideal_DH_IK(parameter, x_um, y_um, z_um, rx_deg, ry_deg, rz_deg, &theta1_deg,
              &d2_m, &d3_m, &d4_m, &theta5_deg, &theta6_deg);

  /* 脉冲换算（与 Matlab 的 main 部分一致） */
  double pulse_theta1_d =
      ZERO_PULSE_THETA1 + theta1_deg * ROTATIONAL_PULSE_PER_DEG_DD;
  double pulse_d2_d = ZERO_PULSE_D2 + d2_m * POSITION_PULSE_PER_M;
  double pulse_d3_d = ZERO_PULSE_D3 + d3_m * POSITION_PULSE_PER_M;
  double pulse_d4_d = ZERO_PULSE_D4 + d4_m * POSITION_PULSE_PER_M;
  double pulse_theta5_d =
      ZERO_PULSE_THETA5 + theta5_deg * ROTATIONAL_PULSE_PER_DEG_DD;
  double pulse_theta6_d =
      ZERO_PULSE_THETA6 + (theta6_deg)*ROTATIONAL_PULSE_PER_DEG_ZOLIX;

  if (pulse_theta1)
    *pulse_theta1 = (int)round(pulse_theta1_d);
  if (pulse_d2)
    *pulse_d2 = (int)round(pulse_d2_d);
  if (pulse_d3)
    *pulse_d3 = (int)round(pulse_d3_d);
  if (pulse_d4)
    *pulse_d4 = (int)round(pulse_d4_d);
  if (pulse_theta5)
    *pulse_theta5 = (int)round(pulse_theta5_d);
  if (pulse_theta6)
    *pulse_theta6 = (int)round(pulse_theta6_d);

  return IK_SUCCESS;
}

/* 新增：增量式逆运动学接口 */
int incremental_inverse_kinematics(const double parameter[6],
                                   const double parameter_FK[6], double x_um,
                                   double y_um, double z_um, double rx_deg,
                                   double ry_deg, double rz_deg,
                                   int *pulse_theta1, int *pulse_d2,
                                   int *pulse_d3, int *pulse_d4,
                                   int *pulse_theta5, int *pulse_theta6) {
  double theta1_deg = 0.0, theta5_deg = 0.0, theta6_deg = 0.0;
  double d2_m = 0.0, d3_m = 0.0, d4_m = 0.0;

  incremental_ideal_DH_IK(parameter, parameter_FK, x_um, y_um, z_um, rx_deg,
                          ry_deg, rz_deg, &theta1_deg, &d2_m, &d3_m, &d4_m,
                          &theta5_deg, &theta6_deg);

  /* 脉冲换算 */
  double pulse_theta1_d =
      ZERO_PULSE_THETA1 + theta1_deg * ROTATIONAL_PULSE_PER_DEG_DD;
  double pulse_d2_d = ZERO_PULSE_D2 + d2_m * POSITION_PULSE_PER_M;
  double pulse_d3_d = ZERO_PULSE_D3 + d3_m * POSITION_PULSE_PER_M;
  double pulse_d4_d = ZERO_PULSE_D4 + d4_m * POSITION_PULSE_PER_M;
  double pulse_theta5_d =
      ZERO_PULSE_THETA5 + theta5_deg * ROTATIONAL_PULSE_PER_DEG_DD;
  double pulse_theta6_d =
      ZERO_PULSE_THETA6 + (theta6_deg)*ROTATIONAL_PULSE_PER_DEG_ZOLIX;

  if (pulse_theta1)
    *pulse_theta1 = (int)round(pulse_theta1_d);
  if (pulse_d2)
    *pulse_d2 = (int)round(pulse_d2_d);
  if (pulse_d3)
    *pulse_d3 = (int)round(pulse_d3_d);
  if (pulse_d4)
    *pulse_d4 = (int)round(pulse_d4_d);
  if (pulse_theta5)
    *pulse_theta5 = (int)round(pulse_theta5_d);
  if (pulse_theta6)
    *pulse_theta6 = (int)round(pulse_theta6_d);

  return IK_SUCCESS;
}